﻿/*
 * Copyright (c) 2010, Shimmer Research, Ltd.
 * All rights reserved
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:

 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer in the documentation and/or other materials provided
 *       with the distribution.
 *     * Neither the name of Shimmer Research, Ltd. nor the names of its
 *       contributors may be used to endorse or promote products derived
 *       from this software without specific prior written permission.

 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * @author Mike Healy
 * @date   January, 2011
 */

using System;
using System.Collections.Generic;
using System.Text;

namespace ShimmerConnect
{
    public class Shimmer
    {
        public static readonly string ApplicationName = "ShimmerConnect";

        public enum SamplingRates
        {
            Sampling1000Hz =    1,
            Sampling500Hz =     2,
            Sampling250Hz =     4,
            Sampling200Hz =     5,
            Sampling166Hz =     6,
            Sampling125Hz =     8,
            Sampling100Hz =     10,
            Sampling50Hz =      20,
            Sampling10Hz =      100,
            Sampling0HzOff =    255
        }

        public static readonly string[] SamplingRatesString = new string[] {
            "   0Hz (Off)",
            " 10Hz",
            " 50Hz",
            "100Hz",
            "125Hz",
            "166Hz",
            "200Hz",
            "250Hz",
            "500Hz",
            "    1kHz"};

        public enum PacketType : byte
        {
            DataPacket                  = 0x00,
            InquiryCommand              = 0x01,
            InquiryResponse             = 0x02,
            GetSamplingRateCommand      = 0x03,
            SamplingRateResponse        = 0x04,
            SetSamplingRateCommand      = 0x05,
            ToggleLedCommand            = 0x06,
            StartStreamingCommand       = 0x07,
            SetSensorsCommand           = 0x08,
            SetAccelSensitivityCommand  = 0x09,
            AccelSensitivityResponse    = 0x0A,
            GetAccelSensitivityCommand  = 0x0B,
            Set5VRegulatorCommand       = 0x0C,
            SetPowerMuxCommand          = 0x0D,
            SetConfigSetupByte0Command  = 0x0E,
            ConfigSetupByte0Response    = 0x0F,
            GetConfigSetupByte0Command  = 0x10,
            StopStreamingCommand        = 0x20, 
            AckCommandProcessed         = 0xFF
        };

        public enum ChannelContents
        {
            XAccel  = 0x00,
            YAccel  = 0x01,
            ZAccel  = 0x02,
            XGyro   = 0x03,
            YGyro   = 0x04,
            ZGyro   = 0x05,
            XMag    = 0x06,
            YMag    = 0x07,
            ZMag    = 0x08,
            EcgRaLl = 0x09,
            EcgLaLl = 0x0A,
            GsrLo   = 0x0B,
            GsrHi   = 0x0C,
            Emg1    = 0x0D,
            Emg2    = 0x0E,
            AnExA0  = 0x0F,
            AnExA7  = 0x10
        }

        public enum SensorBitmap
        {
            SensorAccel     = 0x80,
            SensorGyro      = 0x40,
            SensorMag       = 0x20,
            SensorECG       = 0x10,
            SensorEMG       = 0x08,
            SensorGSR       = 0x04,
            SensorAnExA7    = 0x02,
            SensorAnExA0    = 0x01
        }

        public enum ConfigSetupByte0Bitmap
        {
            Config5VReg = 0x80,
            ConfigPMux  = 0x40
        }

        public enum AccelSensitivity
        {
            RANGE_1_5G = 0,
            RANGE_2_0G = 1,
            RANGE_4_0G = 2,
            RANGE_6_0G = 3
        }

        public enum MaxPacketSizes
        {
            DataPacketSize      = 25,
            ResponsePacketSize  = 17,
            MaxCommandArgSize   = 1
        }

        public static readonly string[] AccelSensitivityString = new string[] {
            "±1.5g",
            "±2g",
            "±4g",
            "±6g"};
    }

    public class ShimmerDataPacket
    {

        private int timeStamp;
        private List<int> channels = new List<int>();
        private int numChannels;
        private bool isFilled = false;

        public ShimmerDataPacket(List<byte> packet, int numChans)
        {
            numChannels = numChans;
            if (packet.Count >= (2 + (numChannels * 2)))                    // timestamp + channel data
            {
                timeStamp = (int)packet[0];
                timeStamp += ((int)packet[1] << 8) & 0xFF00;
                for (int i = 0; i < numChannels; i++)
                {
                    channels.Add((int)packet[2 + (2 * i)]);
                    channels[i] += ((int)packet[2 + (2 * i) + 1] << 8) & 0xFF00;
                }
                isFilled = true;
            }
        }

        public int GetTimeStamp()
        {
            return timeStamp;
        }

        public int GetNumChannels()
        {
            return numChannels;
        }
        public int GetChannel(int channelNum)
        {
            // channelNum is indexed from 0
            if (channelNum >= numChannels)
                return -1;
            else
                return channels[channelNum];
        }

        public bool GetIsFilled()
        {
            return isFilled;
        }
    }

    public class ShimmerProfile
    {
        public bool changeSamplingRate;
        public bool changeSensors;
        public bool change5Vreg;
        public bool changePwrMux;
        public bool changeAccelSens;

        private int adcSamplingRate;
        private int numChannels;
        private int bufferSize;
        private List<int> channels = new List<int>();

        private int sensors;
        private int accelSensitivity;
        private int configSetupByte0;

        private bool isFilled;

        public ShimmerProfile()
        {
            adcSamplingRate = -1;
            numChannels = 0;
            bufferSize = 0;
            isFilled = false;
            sensors = 0;
            accelSensitivity = 0;
            configSetupByte0 = 0;

            changeSamplingRate = false;
            changeSensors = false;
            change5Vreg = false;
            changePwrMux = false;
            changeAccelSens = false;
        }

        public ShimmerProfile(List<byte> packet)
        {
            isFilled = false;
            accelSensitivity = 0;
            configSetupByte0 = 0;

            changeSamplingRate = false;
            changeSensors = false;
            change5Vreg = false;
            changePwrMux = false;
            changeAccelSens = false;

            fillProfile(packet);
        }

        public void fillProfile(List<byte> packet)
        {
            //check if this packet is sane, and not just random
            if ((packet.Count >= 5) // minimum size
                && (packet.Count < (int)Shimmer.MaxPacketSizes.ResponsePacketSize) // maximum size
                && ((int)packet[3] <= 11) // max number of channels currently allowable (3 accel, 3 gyro, 3 mag, 2 AnEx)
                && (Enum.IsDefined(typeof(Shimmer.SamplingRates), (int)packet[0])) // ensure sampling rate is an allowable value
                && (Enum.IsDefined(typeof(Shimmer.AccelSensitivity), (int)packet[1]))) // ensure accel sensitivity is an allowable value
            {
                adcSamplingRate = (int)packet[0];
                accelSensitivity = (int)packet[1];
                configSetupByte0 = (int)packet[2];
                numChannels = (int)packet[3];
                bufferSize = (int)packet[4];

                channels.Clear();

                for (int i = 0; i < numChannels; i++)
                {
                    channels.Add((int)packet[5 + i]);
                }
                isFilled = true;

                updateSensorsFromChannels();
            }
        }

        public int GetAdcSamplingRate()
        {
            return adcSamplingRate;
        }

        public void SetAdcSamplingRate(int rate)
        {
            if(Enum.IsDefined(typeof(Shimmer.SamplingRates), rate))
                adcSamplingRate = rate;
        }

        public int GetBufferSize()
        {
            return bufferSize;
        }

        public void SetBufferSize(int size)
        {
            bufferSize = size;
        }

        public int GetNumChannels()
        {
            return numChannels;
        }

        public void SetNumChannels(int num)
        {
            numChannels = num;
        }

        public int GetChannel(int channelNum)
        {
            if (channelNum >= numChannels)
                return -1;
            else
                return channels[channelNum];
        }

        public bool GetIsFilled()
        {
            return isFilled;
        }

        public void SetVReg(bool val)
        {
            //vReg = val;
            if (val)
            {
                configSetupByte0 |= (int)Shimmer.ConfigSetupByte0Bitmap.Config5VReg;
            }
            else
            {
                configSetupByte0 &= ~(int)Shimmer.ConfigSetupByte0Bitmap.Config5VReg;   
            }
        }

        public bool GetVReg()
        {
            //return vReg;
            if ((configSetupByte0 & (int)Shimmer.ConfigSetupByte0Bitmap.Config5VReg) == 0)
            {
                return false;
            }
            else
            {
                return true;
            }
        }

        public void SetPMux(bool val)
        {
            if (val)
            {
                configSetupByte0 |= (int)Shimmer.ConfigSetupByte0Bitmap.ConfigPMux;
            }
            else
            {
                configSetupByte0 &= ~(int)Shimmer.ConfigSetupByte0Bitmap.ConfigPMux;
            }
        }

        public bool GetPMux()
        {
            //return pMux;
            if ((configSetupByte0 & (int)Shimmer.ConfigSetupByte0Bitmap.ConfigPMux) == 0)
            {
                return false;
            }
            else
            {
                return true;
            }
        }

        public void setSensors(int val)
        {
            sensors = val;
        }

        public int GetSensors()
        {
            return sensors;
        }

        public void SetAccelSensitivity(int val)
        {
            accelSensitivity = val;
        }

        public int GetAccelSensitivity()
        {
            return accelSensitivity;
        }

        public void SetConfigSetupByte0(int val)
        {
            configSetupByte0 = val;
        }

        public int SetConfigSetupByte0()
        {
            return configSetupByte0;
        }

        public void updateSensorsFromChannels()
        {
            // set the sensors value
            // crude way of getting this value, but allows for more customised firmware
            // to still work with this application
            // e.g. if any axis of the accelerometer is being transmitted, then it will
            // recognise that the accelerometer is being sampled
            sensors = 0;
            foreach (int channel in channels)
            {
                if ((channel == (int)Shimmer.ChannelContents.XAccel) ||
                    (channel == (int)Shimmer.ChannelContents.YAccel) ||
                    (channel == (int)Shimmer.ChannelContents.ZAccel))
                {
                    sensors |= (int)Shimmer.SensorBitmap.SensorAccel;
                }
                else if ((channel == (int)Shimmer.ChannelContents.XGyro) ||
                         (channel == (int)Shimmer.ChannelContents.YGyro) ||
                         (channel == (int)Shimmer.ChannelContents.ZGyro))
                {
                    sensors |= (int)Shimmer.SensorBitmap.SensorGyro;
                }
                else if ((channel == (int)Shimmer.ChannelContents.XMag) ||
                         (channel == (int)Shimmer.ChannelContents.YMag) ||
                         (channel == (int)Shimmer.ChannelContents.ZMag))
                {
                    sensors |= (int)Shimmer.SensorBitmap.SensorMag;
                }
                else if ((channel == (int)Shimmer.ChannelContents.EcgLaLl) ||
                         (channel == (int)Shimmer.ChannelContents.EcgRaLl))
                {
                    sensors |= (int)Shimmer.SensorBitmap.SensorECG;
                }
                else if ((channel == (int)Shimmer.ChannelContents.Emg1) ||
                         (channel == (int)Shimmer.ChannelContents.Emg2))
                {
                    sensors |= (int)Shimmer.SensorBitmap.SensorEMG;
                }
                else if ((channel == (int)Shimmer.ChannelContents.GsrHi) ||
                         (channel == (int)Shimmer.ChannelContents.GsrLo))
                {
                    sensors |= (int)Shimmer.SensorBitmap.SensorGSR;
                }
                else if (channel == (int)Shimmer.ChannelContents.AnExA0)
                {
                    sensors |= (int)Shimmer.SensorBitmap.SensorAnExA0;
                }
                else if (channel == (int)Shimmer.ChannelContents.AnExA7)
                {
                    sensors |= (int)Shimmer.SensorBitmap.SensorAnExA7;
                }
            }
        }

        public void updateChannelsFromSensors()
        {
            channels.Clear();

            if ((sensors & (int)Shimmer.SensorBitmap.SensorAccel) != 0)
            {
                channels.Add((int)Shimmer.ChannelContents.XAccel);
                channels.Add((int)Shimmer.ChannelContents.YAccel);
                channels.Add((int)Shimmer.ChannelContents.ZAccel);
            }
            if ((sensors & (int)Shimmer.SensorBitmap.SensorGyro) != 0)
            {
                channels.Add((int)Shimmer.ChannelContents.XGyro);
                channels.Add((int)Shimmer.ChannelContents.YGyro);
                channels.Add((int)Shimmer.ChannelContents.ZGyro);
            }
            if ((sensors & (int)Shimmer.SensorBitmap.SensorECG) != 0)
            {
                channels.Add((int)Shimmer.ChannelContents.EcgRaLl);
                channels.Add((int)Shimmer.ChannelContents.EcgLaLl);
            }
            if ((sensors & (int)Shimmer.SensorBitmap.SensorEMG) != 0)
            {
                channels.Add((int)Shimmer.ChannelContents.Emg1);
                channels.Add((int)Shimmer.ChannelContents.Emg2);
            }
            if ((sensors & (int)Shimmer.SensorBitmap.SensorGSR) != 0)
            {
                channels.Add((int)Shimmer.ChannelContents.GsrLo);
                channels.Add((int)Shimmer.ChannelContents.GsrHi);
            }
            if ((sensors & (int)Shimmer.SensorBitmap.SensorAnExA7) != 0)
            {
                channels.Add((int)Shimmer.ChannelContents.AnExA7);
            }
            if ((sensors & (int)Shimmer.SensorBitmap.SensorAnExA0) != 0)
            {
                channels.Add((int)Shimmer.ChannelContents.AnExA0);
            }
            if ((sensors & (int)Shimmer.SensorBitmap.SensorMag) != 0)
            {
                channels.Add((int)Shimmer.ChannelContents.XMag);
                channels.Add((int)Shimmer.ChannelContents.YMag);
                channels.Add((int)Shimmer.ChannelContents.ZMag);
            }
            numChannels = channels.Count;
        }
    }
}
